#include <stdlib.h>
#include <stdio.h>
#include <p89lpc9351.h>
#include "lcd.h"
#include "adc.h"
#include "timer.h"
#include "pid.h"
#include "configuration.h"
#include "eventDetection.h"

void main(void)
{
    char diff = 0;
    unsigned long startTime = 0; // time when the robot crossed the start line    
    char lcdTimer = 100;
    int state = STRAIGHT; //Drive state variable. Permissible values: STRAIGHT, LEFT, RIGHT, and STOP
    long timeTurn = TURN_TIMER_FULL; //Timer controlling intersection turn
    char startCounter = 0;
    
    // clock must be inited first!
    InitClock();
    InitLCD();
    InitADC();
    InitPID();
    InitMotors();
    
    while(1)
    {
        diff = getDiff();
        //Update rover driving based on current state
        switch (state)
        {
        case STRAIGHT: //If going straight (initialized value)
            pidUpdate(diff, PROPORTIONAL, DERIVATIVE, PID_SPEED);
            state = eventManagerUpdate(diff); //Look for turn signals
            break;
        case LEFT: //Left turn signal detected
            if (timeTurn == TURN_TIMER_FULL) //Turning is about to happen, but not yet
            {
                if (crossDetection())  //Intersection detected!
                    timeTurn -= diff;  //decrement the turn timer
                else //Have not reached the intersection yet
                    pidUpdate(diff, PROPORTIONAL, DERIVATIVE, PID_SPEED); //Keeping driving forward
            }
            else if (timeTurn > diff && timeTurn < TURN_TIMER_FULL)  //The turn timer is counting down, rover performs turn
            {
                  setMotorSpeed(MOTOR_LEFT,  LEFT_TURN_LEFTSPEED);
                  setMotorSpeed(MOTOR_RIGHT, LEFT_TURN_RIGHTSPEED);
                  timeTurn -= diff;
            }
            else //Turn timer reaches 0. Rover steers straight and reload the timer for the next turn
            {
                state = STRAIGHT;
                
                timeTurn = TURN_TIMER_FULL;
            }
            break;
        case RIGHT:
            if (timeTurn == TURN_TIMER_FULL)
            {
                if (crossDetection())
                    timeTurn -= diff;
                else
                    pidUpdate(diff, PROPORTIONAL, DERIVATIVE, PID_SPEED);
            }
            else if (timeTurn > diff && timeTurn < TURN_TIMER_FULL)
            {
                setMotorSpeed(MOTOR_LEFT,  RIGHT_TURN_LEFTSPEED);
                setMotorSpeed(MOTOR_RIGHT, RIGHT_TURN_RIGHTSPEED);
                timeTurn -= diff;
            }
            else
            {
                state = STRAIGHT;
                timeTurn = TURN_TIMER_FULL;
            }
            break;
        case STOP:
            //setMotorSpeed(MOTOR_LEFT, 0);
            //setMotorSpeed(MOTOR_RIGHT, 0);
            state = STRAIGHT;
            break;
        case START:
            if (startCounter == 0)
            {    
                state = STRAIGHT;
                startTime = getMillis();
                startCounter++;
            }
            else
            {
                if (startCounter != 5)
                    startTime = getMillis() - startTime;
                startCounter = 5;
                setMotorSpeed(MOTOR_LEFT, 0);
                setMotorSpeed(MOTOR_RIGHT, 0);
            }
            break;
        }

        if (lcdTimer <= diff)
        {
            lcdTimer = 100;

            lcdClear();
            setLcdHome();
            writeString(" ");
            writeNum(getAnalog(LEFT_SENSOR));
            writeString(" ");
            writeNum(getAnalog(RIGHT_SENSOR));
            writeString(" ");
            writeNum(getAnalog(MID_SENSOR));
            writeString(" ");
            writeNum(lspeed);
            writeString(" ");
            writeNum(rspeed);
            
            setLcdCursor(1, 0);
            writeNum(state);
            writeString(" ");
            writeNum(startCounter == 5 ? startTime : getMillis() - startTime);
            writeString(" ");
            writeNum(getAnalog(BATTERY_SENSOR)*136);
        }
        else lcdTimer -= diff;
        
        delay(1);
    }
}